#!/usr/bin/env python
from numpy import *
from math import *
from vis import *
from threading import Thread
from threading import Event
import time

class link:
	def __init__(self,dhparams,name,parent,visualparams=(10.0,1.0,1.0,1.0),dodraw=True):
		self.a,self.alpha,self.d,self.theta=dhparams
		self.name=name
		self.parent=parent
		self.dodraw=dodraw
		self.size=visualparams[0]
		self.color=visualparams[1:4]

		#transformation matrix from this link to the previous
		self.T=matrix([  [cos(radians(self.theta)), -sin(radians(self.theta))*cos(radians(self.alpha)), sin(radians(self.theta))*sin(radians(self.alpha)), self.a*cos(radians(self.theta))],
				[sin(radians(self.theta)), cos(radians(self.theta))*cos(radians(self.alpha)), -cos(radians(self.theta))*sin(radians(self.alpha)), self.a*sin(radians(self.theta))],
				[0, sin(radians(self.alpha)), cos(radians(self.alpha)), self.d],
				[0, 0, 0, 1]  ])

	def getPos(self):
		pos=self.getT()*matrix([[0],[0],[0],[1]])
		return pos

	def getRot(self):
		T=self.getT() 
		if abs(T[2,0]-1.0)>10.0**(-12.0) and abs(T[2,0]+1.0)>10.0**(-12.0):
			pitch=degrees(-asin(T[2,0]))
			roll=degrees(atan2(T[2,1]/cos(-asin(T[2,0])),T[2,2]/cos(-asin(T[2,0]))))
			yaw=degrees(atan2(T[1,0]/cos(-asin(T[2,0])),T[0,0]/cos(-asin(T[2,0]))))
		else:
			yaw=0;
			delta=degrees(atan2(T[0,1],T[0,2]))

			if abs(T[2,0]+1.0)<10.0**(-12.0):
				pitch=degrees(pi/2.0)
				roll=yaw+delta
			else:
				pitch=degrees(-pi/2.0)
				roll=-yaw+delta
		return (roll,pitch,yaw)
			

	def update(self):
		self.T=matrix([  [cos(radians(self.theta)), -sin(radians(self.theta))*cos(radians(self.alpha)), sin(radians(self.theta))*sin(radians(self.alpha)), self.a*cos(radians(self.theta))],
				[sin(radians(self.theta)), cos(radians(self.theta))*cos(radians(self.alpha)), -cos(radians(self.theta))*sin(radians(self.alpha)), self.a*sin(radians(self.theta))],
				[0, sin(radians(self.alpha)), cos(radians(self.alpha)), self.d],
				[0, 0, 0, 1]  ])
	def getT(self):
		if self.parent==None:
			return self.T
		
		T=self.parent.getT()*self.T
		return T

	def draw(self):
		if not self.a== 0 and self.dodraw==True:
			glPushMatrix()
			T=self.getT()
			#glMatrix=[1,0,0,0,
			#	  0,1,0,0,
			#	  0,0,1,0,
			#	  0,0,0,1]
			glMultMatrixd([T[0,0], T[1,0], T[2,0], 0., 
				       T[0,1], T[1,1], T[2,1], 0.,
				       T[0,2], T[1,2], T[2,2], 0.,
				       T[0,3], T[1,3], T[2,3], 1.0])
			glTranslatef(-self.a/2.0, 0.0,0.0)
			glScale(self.a-10.0, self.size, self.size)
			glColor3f(self.color[0],self.color[1],self.color[2])
			glutSolidCube(1)
			glPopMatrix()



class engine(Thread):
	def __init__(self,links):
		Thread.__init__(self)
		self.links=links
		self.running=True
		self.UpdateEvent=Event()


	def run(self):
		print("Engine Started")
		while self.running:
			self.UpdateEvent.wait()
			#print("UpdateEvent set!!")			
			for link in self.links:
				link.update()

			self.UpdateEvent.clear()












